#pragma once

#include "CoordinateSys.h"
#include "CartesianCRD.hpp"

namespace ns_Position
{
    // 大地坐标
    class GeodeticCRD : public CoordinateSys
    {
    public:
        using PositionType = double;

    public:
        GeodeticCRD()
        {}

        GeodeticCRD(PositionType latitude, PositionType longitude, PositionType height)
            : _latitude(latitude),
              _longitude(longitude),
              _height(height)
        {}

        bool ToCartesianCRD(CartesianCRD* out) const override
        {
            CartesianCRD::PositonType x = ToCarCRDX(_latitude, _longitude, _height);
            CartesianCRD::PositonType y = ToCarCRDY(_latitude, _longitude, _height);
            CartesianCRD::PositonType z = ToCarCRDZ(_latitude, _longitude, _height);

            CartesianCRD *ans = new CartesianCRD(x, y, z);
            out = ans;

            return true;
        }

        bool FromCartesianCRD(const CartesianCRD& ref) override
        {
            CartesianCRD::PositonType x = ref.GetX();
            CartesianCRD::PositonType y = ref.GetY();
            CartesianCRD::PositonType z = ref.GetZ();

            _latitude = ToLatitude(x, y, z);
            _longitude = ToLongitude(x, y, z);
            _height = ToHeight(x, y, z);

            return true;
        }

        // 获取纬度
        PositionType GetB() const
        {
            return _latitude;
        }

        // 获取经度
        PositionType GetL() const
        {
            return _longitude;
        }

        // 获取大地高
        PositionType GetH() const
        {
            return _height;
        }

    private:
        PositionType _latitude;  // 纬度B
        PositionType _longitude; // 经度L
        PositionType _height;    // 大地高H

        CartesianCRD::PositonType ToCarCRDX(double B, double L, double H) const
        {
            CartesianCRD::PositonType CurvatureRadius = Earth::LongAxis / Mathf::Pow(1 - Mathf::Pow(Earth::Eccentricity * Mathf::Sin(B * Mathf::Pi / 180), 2), 0.5);
            return (CurvatureRadius + H) * Mathf::Cos(B * Mathf::Pi / 180) * Mathf::Cos(L * Mathf::Pi / 180);
        }
        CartesianCRD::PositonType ToCarCRDY(double B, double L, double H) const
        {
            CartesianCRD::PositonType CurvatureRadius = Earth::LongAxis / Mathf::Pow(1 - Mathf::Pow(Earth::Eccentricity * Mathf::Sin(B * Mathf::Pi / 180), 2), 0.5);
            return (CurvatureRadius + H) * Mathf::Cos(B * Mathf::Pi / 180) * Mathf::Sin(L * Mathf::Pi / 180);
        }
        CartesianCRD::PositonType ToCarCRDZ(double B, double L, double H) const
        {
            CartesianCRD::PositonType CurvatureRadius = Earth::LongAxis / Mathf::Pow(1 - Mathf::Pow(Earth::Eccentricity * Mathf::Sin(B * Mathf::Pi / 180), 2), 0.5);
            return (CurvatureRadius * (1 - Mathf::Pow(Earth::Eccentricity, 2)) + H) * Mathf::Sin(B * Mathf::Pi / 180);
        }

        PositionType ToLatitude(CartesianCRD::PositonType X, CartesianCRD::PositonType Y, CartesianCRD::PositonType Z)
        {
            double R = Mathf::Sqrt(X * X + Y * Y);
            double B1 = Mathf::Arctan(R, Z);
            double B2;
            while (1)
            {
                double W1 = Mathf::Sqrt(1 - Earth::Eccentricity * Earth::Eccentricity * Mathf::Sin(B1) * Mathf::Sin(B1));
                double N1 = Earth::LongAxis / W1;
                B2 = Mathf::Arctan((Z + N1 * Earth::Eccentricity * Earth::Eccentricity * Mathf::Sin(B1)) / R);
                if (Mathf::Abs(B2 - B1) <= 1e-11)
                    break;
                B1 = B2;
            }
            return B2 * 180 / Mathf::Pi;
        }

        PositionType ToLongitude(CartesianCRD::PositonType X, CartesianCRD::PositonType Y, CartesianCRD::PositonType Z)
        {
            return Mathf::Arctan(X, Y) * 180 / Mathf::Pi;
        }

        PositionType ToHeight(CartesianCRD::PositonType X, CartesianCRD::PositonType Y, CartesianCRD::PositonType Z)
        {
            double B = ToLatitude(X, Y, Z);
            double N = Earth::LongAxis / Mathf::Sqrt(1 - Mathf::Pow(Earth::Eccentricity * Mathf::Sin(B * Mathf::Pi / 180), 2));
            double FAI = Mathf::Arctan(Z / Mathf::Pow(Mathf::Pow(X, 2) + Mathf::Pow(Y, 2), 0.5));
            return Mathf::Sqrt(X * X + Y * Y + Z * Z) * Mathf::Cos(FAI) / Mathf::Cos(B * Mathf::Pi / 180) - N;
        }
    };
}